#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"
#include "base_interfaces_demo/srv/patrol.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
using base_interfaces_demo::srv::Patrol;
using geometry_msgs::msg::Twist;
using turtlesim::msg::Pose;
using std::placeholders::_1;
using std::placeholders::_2;
using rcl_interfaces::msg::SetParametersResult;

class TurtlePatrolService:public rclcpp::Node
{
public:
    TurtlePatrolService(const std::string node_name):Node(node_name)
    {
        this->declare_parameter("k",1.0);
        this->declare_parameter("max_speed",1.0);
        this->get_parameter("k",k);
        this->get_parameter("max_speed",max_speed_);

        parameter_callback_handle_=this->add_on_set_parameters_callback
        (
        [&](const std::vector<rclcpp::Parameter> & parameters)
        ->rcl_interfaces::msg::SetParametersResult 
        {
            rcl_interfaces::msg::SetParametersResult result;
            result.successful = true;
            for (const auto & parameter : parameters) 
            {   
                RCLCPP_INFO(this->get_logger(),"更新参数的值%s=%f",
                parameter.get_name().c_str(),
                parameter.as_double());
            
                if (parameter.get_name()=="k") 
                {
                    k=parameter.as_double();
                }
                else if (parameter.get_name()=="max_speed")
                {
                    max_speed_=parameter.as_double();
                }
            }
            return result;
        }
        );

        
        velocity_publisher_=this->create_publisher<Twist>("turtle1/cmd_vel",10);
        pose_subscription_=this->create_subscription<Pose>("turtle1/pose",10,
                        std::bind(&TurtlePatrolService::on_pose_callback,this,_1));
        patrol_service=this->create_service<Patrol>
        ("patrol",
        std::bind(&TurtlePatrolService::call_back,
        this,_1,_2)
        );                
    }

private:
    OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
    void on_pose_callback(const Pose::SharedPtr pose)
    {
        auto message=Twist();
        double current_x=pose->x;
        double current_y=pose->y;
        RCLCPP_INFO(this->get_logger(),"当前位置(x=%f,y=%f)",current_x,current_y);
        double distance=std::sqrt((target_x_-current_x)*(target_x_-current_x)
                                +(target_y_-current_y)*(target_y_-current_y));
        double angle=std::atan2(target_y_-current_y,target_x_-current_x)-pose->theta;

        if(distance>0.1)
        {
            if(fabs(angle)>0.2) //绝对值
            {
                message.angular.z=fabs(angle);
            }
            else
            {
        message.linear.x=k* distance;
            }
        }
        if(message.linear.x>max_speed_)
        {
            message.linear.x=max_speed_;
        }
        velocity_publisher_->publish(message);                        
    }
    void call_back(const std::shared_ptr<Patrol::Request>request,
                    std::shared_ptr<Patrol::Response>response)
    {
        if((0<request->target_x && request->target_x<12.0f)
        &&( 0<request->target_y && request->target_y<12))
        {
            target_x_=request->target_x;
            target_y_=request->target_y;
            response->result=Patrol::Response::SUCCESS;
        }
        else
        {
            response->result=Patrol::Response::FALL;
        }

    }
    rclcpp::Publisher<Twist>::SharedPtr velocity_publisher_;
    rclcpp::Subscription<Pose>::SharedPtr pose_subscription_;
    rclcpp::Service<Patrol>::SharedPtr patrol_service;

    double target_x_=1.0;
    double target_y_=1.0;
    double k=1.0;
    double max_speed_=3.0;
};
int main(int argc,char*argv[])
{
    rclcpp::init(argc,argv);
    rclcpp::spin(std::make_shared<TurtlePatrolService>("turtle_patrol_client_node"));
    rclcpp::shutdown();
    return 0;
}